#include <iostream>
#include <string>

#include "Modeler.h"

using namespace std;
using namespace elisa;

const VNInterval GusBurner_H(2.0);
const VNInterval GusBurner_A(0.01);
const VNInterval GusBurner_B(0.005);

class GusBurner;

// 連続制約の定義
template<typename T> 
void GusBurner_dfunc(int n, T *dv, const T *v, T t, void *param)
{
	if (GusBurner::SW1 && !GusBurner::SW2) {
		dv[0] = GusBurner_H - GusBurner_A*v[0] + GusBurner_B*v[1];
		dv[1] = -GusBurner_A*v[1] + GusBurner_B*v[0];
	} else if (!GusBurner::SW1 && !GusBurner::SW2) {
		dv[0] = -GusBurner_A*v[0] + GusBurner_B*v[1];
		dv[1] = -GusBurner_A*v[1] + GusBurner_B*v[0];
	} else if (!GusBurner::SW1 && GusBurner::SW2) {
		dv[0] = -GusBurner_A*v[0] + GusBurner_B*v[1];
		dv[1] = GusBurner_H - GusBurner_A*v[1] + GusBurner_B*v[0];
	}
}

// ガード制約の定義
template<typename T> 
T GusBurner_gfunc(const std::vector<T>& v, const T& t)
{
	return v[0] - 100;
	//return v[1] - 100;
	//return v[0] - 80;
	//return v[1] - 80;
}


class GusBurner : public Modeler
{
public:
	static bool SW1;
	static bool SW2;

private:
	Time t;
	ContVar x;
	ContVar y;
	ContVarVec cvv;
	std::vector<RealExpr> rev;

	static Interval nextT;
	static VNInterval nextX, nextY;

public:
	GusBurner()
		: x (nextX, "x"),
		  y (nextY, "y"),
		  //t(nextT, nextT.width()*2)
		  t(nextT, 0)
	{
		double p((nextT.width() + nextX.width() + nextY.width()) / 3);
		if (p != 0.0)
			elisa::RealVar::SetPrecision(p);
		else
			elisa::RealVar::SetPrecision(1.0e-1);
		// FIXME
		elisa::RealVar::SetPrecision(1.0e-1);

		p = 1.0e-1;
		//p = 1.0e-11;
		t.setPrecision(std::max<double>(p, nextT.width()*2));

		t.setDomain(new Interval(nextT.inf(), 100.0));

		cvv.push_back(&x);
		cvv.push_back(&y);
		t.addInitValue(cvv);

		rev.push_back(x);
		rev.push_back(y);

#ifdef _DEBUG
cout << "t0: " << nextT << ", x0: " << nextX << ", y0: " << nextY << endl;
#endif
	}

	const ContConstraint cc() 
	{
		return ContConstraint(new ContConstraintRep(cvv, t, GusBurner_dfunc, GusBurner_dfunc),
			GusBurner_gfunc<VNInterval>, GusBurner_gfunc<AD_VNInterval>, GusBurner_dfunc<VNInterval>);
	}

	const RealConstraint gc() 
	{
		return RealConstraint(GusBurner_gfunc<RealExpr>(rev, t) == 0);
	}

	bool reset(bool first)
	{
		Interval nt(t.domain().inf(), t.domain().sup());
		VNInterval nx(interval(x.domain().inf(), x.domain().sup()));
		VNInterval ny(interval(y.domain().inf(), y.domain().sup()));

		if (first) { // 最初に見つかった解
			if (nextT.overlaps(nt))
				// 前フェーズの終了時間と重なっていた場合は更新しない
				return false;

			nextT = nt;
			nextX = nx;
			nextY = ny;
		
		} else { // 2つ目以降

			if (!nextT.overlaps(nt))
				// これまでの解と重なりがない → 定性的に異なる解 (TODO)
				return false;
			
			nextT  = hull(nextT, nt);
			nextX  = elisa::vn::hull(nextX, nx);
			nextY  = elisa::vn::hull(nextY, ny);
		}

		// 和をとった包囲を表示
		cout << '\t' << "nt " << " = " << nextT  << endl;
		cout << '\t' << "nx " << " = " << nextX  << endl;
		cout << '\t' << "ny " << " = " << nextY  << endl;

		return true;
	}
	
	void print()
	{
		cout << '\t' << t.getName() << " = " << t.domain() << endl;
		//cout << '\t' << y.getName() << " = " << y.domain() << endl;
		cout << '\t' << "x " << " = " << x.domain() << endl;
		cout << '\t' << "y " << " = " << y.domain() << endl;
	}
};


Interval GusBurner::nextT(Interval::Zero);
// 1
bool GusBurner::SW1(true);
bool GusBurner::SW2(false);
VNInterval GusBurner::nextX(0.0);
VNInterval GusBurner::nextY(50.0);
// 2
//bool GusBurner::SW1(false);
//bool GusBurner::SW2(true);
//VNInterval GusBurner::nextX(99.999999996937177, 100.00000000000024);
//VNInterval GusBurner::nextY(40.977283188413864, 40.977283188643348);
// 3
//bool GusBurner::SW1(false);
//bool GusBurner::SW2(false);
//VNInterval GusBurner::nextX(81.744110137865349, 81.744110140222915);
//VNInterval GusBurner::nextY(99.999999999456122, 100.00000000053987);
// 4
//bool GusBurner::SW1(true);
//bool GusBurner::SW2(false);
//VNInterval GusBurner::nextX(79.999999997739508, 80.000000002274959);
//VNInterval GusBurner::nextY(96.808631649668129, 96.808631654846195);
// 5
//bool GusBurner::SW1(false);
//bool GusBurner::SW2(false);
//VNInterval GusBurner::nextX(99.999999995708961, 100.00000000429105);
//VNInterval GusBurner::nextY(90.621777370318426, 90.621777376339651);
// 6
//bool GusBurner::SW1(false);
//bool GusBurner::SW2(true);
//VNInterval GusBurner::nextX(86.217781208736042, 86.217781222865298);
//VNInterval GusBurner::nextY(79.999999994481854, 80.000000005518174);
// 6
//bool GusBurner::SW1(false);
//bool GusBurner::SW2(false);
//VNInterval GusBurner::nextX(81.149617086750951, 81.14961710212792);
//VNInterval GusBurner::nextY(99.999999989489382, 100.0000000105106);
